#include "robot_status_client.h"
#include <QEventLoop>
#include <QTimer>
#include <QFile>
#include <QDir>
#include <QStandardPaths>
#include "map_download_action_client.h"
#include "../include/global_logger.h"
RobotStatusClient::RobotStatusClient(QObject *parent) 
    : QObject(parent)
    , rclcpp::Node("robot_status_client") 
{
}

RobotStatusClient::~RobotStatusClient()
{
}

// 设置地图下载客户端
void RobotStatusClient::setMapDownloadClient(std::shared_ptr<MapDownloadActionClient> client)
{
    mapDownloadClient_ = client;
    // 连接信号槽，当请求下载地图时调用MapDownloadActionClient的下载方法
    connect(this, &RobotStatusClient::mapDownloadRequested, 
            mapDownloadClient_.get(), &MapDownloadActionClient::onRequestDownloadMap);
}

// 查询所有信息(位置、电量、导航状态)
void RobotStatusClient::queryAll(const QString& robotId)
{
    sendRequest(robotId, rms_pkg::srv::RobotStatus::Request::QUERY_ALL);
}

// 查询位置
void RobotStatusClient::queryPosition(const QString& robotId)
{
    sendRequest(robotId, rms_pkg::srv::RobotStatus::Request::QUERY_POSITION_ONLY);
}

// 查询电量
void RobotStatusClient::queryBattery(const QString& robotId)
{
    sendRequest(robotId, rms_pkg::srv::RobotStatus::Request::QUERY_BATTERY_ONLY);
}

// 查询导航状态
void RobotStatusClient::queryNavigationState(const QString& robotId)
{
    sendRequest(robotId, rms_pkg::srv::RobotStatus::Request::QUERY_NAVIGATION_ONLY);
}

// 查询地图名称
void RobotStatusClient::queryMapName(const QString& robotId)
{
    LOG_DEBUG("查询地图名称 - 机器人:{}",robotId);
    // 使用Qt::QueuedConnection确保在对象自己的线程中执行
    //QMetaObject::invokeMethod(this, [this, robotId]() {
        sendRequest(robotId, rms_pkg::srv::RobotStatus::Request::QUERY_MAP_NAME_ONLY);
    //}, Qt::QueuedConnection);
}

// 获取当前地图名称
QString RobotStatusClient::getCurrentMapName(const QString& robotId)
{
    // 创建针对特定robotId的客户端
    std::string service_name = "/" + robotId.toStdString() + "/get_robot_status";
    auto client = this->create_client<rms_pkg::srv::RobotStatus>(service_name);
    
    if (!client->service_is_ready()) {
        return QString();
    }

    // 创建请求
    auto request = std::make_shared<rms_pkg::srv::RobotStatus::Request>();
    request->query_type = rms_pkg::srv::RobotStatus::Request::QUERY_MAP_NAME_ONLY;
    
    // 同步发送请求并等待结果
    auto result = client->async_send_request(request);
    
    // 使用事件循环等待结果
    QEventLoop loop;
    QTimer timeoutTimer;
    timeoutTimer.setSingleShot(true);
    timeoutTimer.setInterval(5000); // 5秒超时
    
    QString mapName;
    
    // 连接超时信号
    connect(&timeoutTimer, &QTimer::timeout, &loop, &QEventLoop::quit);
    
    // 异步等待结果
    using namespace std::placeholders;
    client->async_send_request(
        request,
        [&loop, &mapName, this](rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future) {
            try {
                auto response = future.get();
                if (response->success) {
                    mapName = QString::fromStdString(response->current_map);
                }
            } catch (...) {
                // 错误处理
            }
            loop.quit();
        });
    
    timeoutTimer.start();
    loop.exec();
    
    // 如果超时，返回空字符串
    if (timeoutTimer.isActive()) {
        timeoutTimer.stop();
        return mapName;
    } else {
        // 超时情况
        return QString();
    }
}

bool RobotStatusClient::isFileExists(const QString &filePath)
{
    return QFile::exists(filePath);
}

// 下载地图
void RobotStatusClient::downloadMap(const QString &robotId, const QString &mapName)
{
    // 创建地图目录
    QString mapDirPath = QString("data/%1/map").arg(robotId);
    QDir mapDir(mapDirPath);
    if (!mapDir.exists()) {
        mapDir.mkpath(".");
    }
    
    // 如果有地图下载客户端，使用它来下载地图
    if (mapDownloadClient_) {
        // 注意：这里需要根据实际的Action接口来实现
        // 这只是一个示例，具体实现取决于Action的定义
        emit mapDownloadRequested(robotId, mapName);
    } else {
        // 如果没有Action客户端，发出信号让其他组件处理
        emit mapDownloadRequested(robotId, mapName);
    }
}

// 添加一个新的公共方法，用于查询地图并根据需要下载
void RobotStatusClient::checkAndDownloadMap(const QString &robotId)
{
    // 查询当前地图名称
    QString mapName = getCurrentMapName(robotId);
    if (!mapName.isEmpty()) {
        // 检查本地是否存在该地图
        QString mapFileName = mapName;
        if (!mapFileName.endsWith(".smap")) {
            mapFileName += ".smap";
        }
        QString mapFilePath = QString("data/%1/map/%2").arg(robotId, mapFileName);
        
        if (!isFileExists(mapFilePath)) {
            // 地图文件不存在，开始下载
            LOG_DEBUG("地图文件不存在，开始下载 - 地图:{} 路径:{}",mapName,mapFilePath);
            downloadMap(robotId, mapName);
        } else {
            LOG_DEBUG("地图文件已存在 - 地图:{}",mapName);
        }
    }
}

/**
 * @brief 向指定机器人的状态服务发送查询请求
 * 
 * 该函数会为指定的机器人ID创建一个服务客户端，连接到对应的机器人状态服务，
 * 然后根据查询类型发送异步请求。请求结果将通过回调函数处理，并最终调用
 * handleResponse函数进行处理。
 * 
 * @param robotId 机器人唯一标识符，用于构建服务名称
 * @param queryType 查询类型，决定需要查询的机器人状态信息类型
 */
void RobotStatusClient::sendRequest(const QString& robotId, int queryType)
{
    // 为特定robotId创建客户端
    std::string service_name = "/" + robotId.toStdString() + "/get_robot_status";
    LOG_DEBUG("创建客户端，服务名称:{}",service_name);
    auto client = this->create_client<rms_pkg::srv::RobotStatus>(service_name);

    // 检查服务是否就绪
    if (!client->service_is_ready()) {
        LOG_DEBUG("服务未就绪，等待服务上线 robotId:{}",robotId);
        bool is_ready = client->wait_for_service(std::chrono::seconds(5));
        if (!is_ready) {
            LOG_DEBUG("等待服务超时 robotId:{}",robotId);
            emit queryFailed("Service timeout for robot: " + robotId);
            return;
        }
    }
    
    LOG_DEBUG("服务已就绪 robotId:{} 查询类型:{}",robotId,queryType);

    // 创建请求
    auto request = std::make_shared<rms_pkg::srv::RobotStatus::Request>();
    request->query_type = queryType;
        
    // 存储客户端到成员变量以保持其生命周期
    // 每个robotId使用独立的客户端
    clients_[service_name] = client;
    
    // 异步发送请求，直接捕获this指针和client
    auto result_future = client->async_send_request(
        request,
        [this, queryType, robotId, service_name, client](rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future) {
            LOG_DEBUG("=== 收到服务端响应 === robotId:{} 查询类型:{}",robotId,queryType);
            try {
                auto response = future.get();
                LOG_DEBUG("响应内容 - 成功:{} 消息:{}",response->success,response->message);
                if (response->success) {
                    LOG_DEBUG("地图名称:{}",response->current_map);
                }
            } catch (const std::exception& e) {
                LOG_DEBUG("获取响应时发生异常:{}",e.what());
            }
            
            this->handleResponse(future, queryType);
            
            // 处理完成后从clients_中移除客户端（可选）
            // this->clients_.erase(service_name);
        });
    
    LOG_DEBUG("异步请求已发送 robotId:{} 查询类型:{}",robotId,queryType);
}

/*
 * @brief 处理机器人状态服务的响应
 * 
 * 根据查询类型处理来自机器人状态服务的响应，并发出相应的信号。
 * 不同的查询类型会触发不同的信号：
 * - QUERY_ALL 或 QUERY_POSITION_ONLY: 发出 positionUpdated 信号
 * - QUERY_ALL 或 QUERY_BATTERY_ONLY: 发出 batteryUpdated 信号
 * - QUERY_ALL 或 QUERY_NAVIGATION_ONLY: 发出 navigationStateChanged 信号
 * - QUERY_ALL 或 QUERY_MAP_NAME_ONLY: 发出 mapNameUpdated 信号
 * 
 * @param future 服务响应的future对象，包含响应数据
 * @param queryType 查询类型，决定需要处理哪些数据并发出相应信号
 */
void RobotStatusClient::handleResponse(rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future, int queryType)
{
    LOG_DEBUG("进入handleResponse方法，查询类型:{}",queryType);
    try {
        auto response = future.get();
        LOG_DEBUG("响应获取成功，成功标志:{}",response->success);
        
        if (response->success) {
            LOG_DEBUG("响应成功，开始根据查询类型发出信号");
            // 根据查询类型发出相应的信号
            switch (queryType) {
            case rms_pkg::srv::RobotStatus::Request::QUERY_ALL:
            case rms_pkg::srv::RobotStatus::Request::QUERY_POSITION_ONLY:
                LOG_DEBUG("发出positionUpdated信号 x:{} y:{} angle:{}",response->x,response->y,response->angle);
                emit positionUpdated(response->x, response->y, response->angle);
                if (queryType == rms_pkg::srv::RobotStatus::Request::QUERY_POSITION_ONLY) break;
                // 如果是QUERY_ALL，继续处理其他数据
                
            case rms_pkg::srv::RobotStatus::Request::QUERY_BATTERY_ONLY:
                LOG_DEBUG("发出batteryUpdated信号 battery:",response->battery);
                emit batteryUpdated(response->battery);
                if (queryType == rms_pkg::srv::RobotStatus::Request::QUERY_BATTERY_ONLY) break;
                
            case rms_pkg::srv::RobotStatus::Request::QUERY_NAVIGATION_ONLY:
                LOG_DEBUG("发出navigationStateChanged信号 state:{} target:{}",response->navigation_state,response->current_target);
                emit navigationStateChanged(
                    QString::fromStdString(response->navigation_state),
                    QString::fromStdString(response->current_target));
                if (queryType == rms_pkg::srv::RobotStatus::Request::QUERY_NAVIGATION_ONLY) break;
                
            case rms_pkg::srv::RobotStatus::Request::QUERY_MAP_NAME_ONLY:
                LOG_DEBUG("发出mapNameUpdated信号 mapName:{}",response->current_map);
                emit mapNameUpdated(QString::fromStdString(response->current_map));
                break;
            }
        } else {
            LOG_DEBUG("响应失败，发出queryFailed信号 错误信息:{}",response->message);
            emit queryFailed(QString::fromStdString(response->message));
        }
    } catch (const std::exception &e) {
        LOG_DEBUG("处理响应时发生异常:{}",e.what());
        emit queryFailed(QString::fromStdString(e.what()));
    }
    LOG_DEBUG("handleResponse方法执行完成");
}